/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file exampleDaemon.cxx 
	\brief Contains initialization and shutdown code for the various daemon threads

		                \author Jack Elston
		                \date   Oct 11 2006
*/
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>

#ifdef USING_XML
 #include "dataToXML.h"
 #include "XMLtoData.h"
#endif

#ifdef USING_GUI
 #include "vcgui3.h"
 #include "vcConfig.h"
#endif

#include "exampleDaemon.h"
#include "socketThread.h"
#include "serialThread.h"
#include "MAVParser.h"
#include "testing.h"
#include "dataDistribution.h"
#include "threadedPipe.h"
#include "interface.h"
#include "networkAppliance.h"
#include "rssiTrace.h"
#include "gpsParser.h"
#include "ipmt.h"
#include "gatewayParser.h"
#include "aodvParser.h"
#include "joystickThread.h"
#include "joystickTranThread.h"
#include "vps.h"
#ifdef HAVE_PICCOLO_SDK
  #include "piccoloParser.h"
#endif
#include "gsBackup.h"
#include "gpsSimParser.h"

	/*<-------Defines--------->*/
	#undef USE_TCP_CMD
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		/*! \brief a global variable. */
		ShmStruct shm;

		String adhocPortStr_udp = "1000";
#ifdef USE_TCP_CMD
		String adhocPortStr_tcp = "1001";
#endif
		String recuvPortStr     = "23456";
		String inetInPortStr    = "56789";
		String inetOutPortStr   = "12345";
		String nodeToTrack      = "";

		String piccoloGSCommIPStr="10.0.0.10";
		String piccoloGSCommPortStr="2000";

		bool gw_adhoc   = true;
		bool gw_net     = true;
		bool internal_net = true;

		bool gw_gps     = false;
		bool gw_rssi    = false;
		bool gw_mav     = false;
		bool gw_recuv   = false;
		bool gw_gui     = false;
		bool gw_ipmt    = false;
		bool gw_monitor = true;
		bool gw_joy     = false;
		bool gw_vps     = false;
		bool gw_piccolo_gs = false;
		bool sim_gps    = false;

#ifdef USING_GUI
		GuiStruct guiStr;
#endif
		String baud             = "57600";
		String mavSerialPortStr = "/dev/ttyS0";

		String gpsBaud          = "4800";
		//String gpsSerialPortStr = "/dev/ttyS0";
		String gpsSerialPortStr = "/dev/ttyUSB0";

		String simGPSSerialPortStr = "/dev/ttyUSB1";

		String joySerialPortStr = "/dev/input/js0";

	/*<-End Global Variables-->*/

/************************************************************\
|                           main                             |
\************************************************************/
/*!
	\brief main function for daemon
	\param argc number of command line arguments
	\param argv character array from command line input
	\pre user must be root
	\post program terminated
	\see exampleDaemon_Usage for usage
	\return The results
	\todo The verbose output needs to be cleaned up
	\todo Vehicle ID field is still not being used
 */
int main(int argc, char *argv[])
{
	/*<----Local Variables---->*/
		extern int optind;
		int c;
		String gpsFile = "";
		String gladeFile = "";
	/*<--End Local Variables-->*/

	// this really should come from an interface selection, and more 
	// importantly the netmask should definetly be gotten from the IF
	internal_subnet.s_addr = inet_addr("6.0.0.0");
	netmask.s_addr = inet_addr("255.255.255.0");
	char name[64];
	if(gethostname(name,64) == EXIT_SUCCESS)
		vehicleID = String(name);

	while ((c=getopt(argc,argv,"ade:hi:l:m:n:o:p:s:uvc:f:rgtbCNAEw:j")) != EOF) switch( (char)c)
	{
		case 'e':
			simGPSSerialPortStr = String(optarg);
			sim_gps = true;
			break;
		case 'a':
			gw_piccolo_gs = true;
			break;
		case 'h':
			ExampleDaemon_Usage(argv[0]);
			exit(EXIT_SUCCESS);
			break;
		case 'i':
			vehicleID = String(optarg);
			break;
		case 'l':
			logDir = String(optarg);
			if( logDir.length() > 0 ) {
				char last = logDir( logDir.length() -1 );
				if( last != '/' ) logDir += "/";
				logging = true;
			}
			break;
		case 'm':
			mavSerialPortStr = String(optarg);
			gw_mav = true;
			break;
		case 'n':
			internal_subnet.s_addr = inet_addr(optarg);
			break;
		case 'o':
			inetOutPortStr = String(optarg);
			break;
		case 'p':
			inetInPortStr = String(optarg);
			break;
		case 'u':
			gw_gui = true;
			gw_vps = true;
			break;
		case 'v':
			verbose = true;
			break;
		case 'f':
			gpsFile = String(optarg);
			break;
		case 'c':
			gladeFile = String(optarg);
			break;
		case 'r':
			gw_recuv = true;
			break;
		case 'g':
			gw_gps = true;
			break;
		case 't':
			gw_rssi = true;
			break;
		case 'b':
			gw_ipmt = true;
			break;
		case 'j':
			gw_joy = true;
			break;
		case 'A':
			gw_adhoc = false;
			break;
		case 'N':
			gw_net = false;
			break;
		case 'E':
			internal_net = false;
			break;
		case 'C':
			gw_monitor = false;
			break;
		case 'w':
			nodeToTrack = String(optarg);
			break;
		case 'd':
			break;
		default:
			ExampleDaemon_Usage(argv[0]);
			exit(EXIT_SUCCESS);
	}

	if(argc < 1) {
		ExampleDaemon_Usage(argv[0]);
		exit(EXIT_SUCCESS);
	}

	if( optind < argc) {
		ExampleDaemon_Usage(argv[0]);
		exit(EXIT_SUCCESS);
	}

	if(vehicleID ==  "") {
		printf("Invalid argument: \"\" is not a valid ID\n");
		ExampleDaemon_Usage(argv[0]);
		exit(EXIT_SUCCESS);
	}

	uid_t uid = getuid();
	uid_t e_uid = geteuid();

	if( uid != e_uid || uid != 0 ) {
		printf("User is %s: id=%d, eid=%d\n\n", getlogin(), uid, e_uid);
		cout << "\t!----- YOU MUST HAVE SUPERUSER PERMISSIONS -----!\n" << endl;
		exit(EXIT_FAILURE);
	}

#ifdef USING_GUI
	// initialize libraries used by gui
	if( gw_gui ) {

		//g_type_init();
		if( !g_thread_supported() ) {
			g_thread_init(NULL);
			gdk_threads_init(); // Called to initialize internal mutex "gdk_threads_mutex".
		} else {
			printf("VCGUI::VCGUI -- g_thread NOT supported\n");
			::exit(EXIT_FAILURE);
		}
		
		// GTK/GDK thread enter
		gdk_threads_enter();

		// Initialize gtk and load style
		gtk_rc_parse("/usr/local/share/vcgui/gtkrc");
		gtk_init (&argc, &argv);

		gdk_rgb_init();

		// GTK/GDK thread leave
		gdk_flush();
		gdk_threads_leave();

		// initialize configuration
		vc_config_init(argc, argv);

		if( gpsFile != "" ){
			if( guiStr.config.map_files )
				g_list_free( guiStr.config.map_files );
			guiStr.config.map_files = NULL;
			if( gpsFile != " " )
				guiStr.config.map_files = g_list_append( guiStr.config.map_files, gpsFile.c_str() );
		}
		if( gladeFile != "" )
			guiStr.config.glade_file = gladeFile;
	}
#endif

	/*<---Initialize Signals-->*/
	signal( SIGINT, ExampleDaemon_Exit);
	signal( SIGQUIT, ExampleDaemon_Exit);

	sigset_t waitSigs;
	sigemptyset(&waitSigs);
	sigaddset(&waitSigs, TSIGQUIT);
	sigaddset(&waitSigs, TSIGTIMER);
	for(int j=SIGRTMIN; j <= SIGRTMAX; j++) 
		sigaddset(&waitSigs, j);
	sigprocmask(SIG_BLOCK,&waitSigs,NULL);


	/*<--------- Daemon ----------->*/
	NetworkAppliance exampleDaemon("exampleDaemon");

	/*<---------GUI----------->*/
#ifdef USING_GUI
	Interface userInterface("User Interface");
	if(gw_gui) {
		userInterface.setCompBlock(new VCGUI("VCGUI",argc,argv));
		userInterface.setSubnet(inet_addr("0.0.0.0"));
#ifdef USING_VPS
		Interface vps("VPS Interface");
		if(gw_vps) {
			vps.setCompBlock(new VPS("VPS",argc,argv));
		}
#endif
	}
#endif

  /*<--------- Simulated GPS ----------->*/
	Interface simgps_interface("Simulated GPS");
	if(sim_gps) {
		simgps_interface.setCommBlock(new SerialThread("GPS Sim Tx", simGPSSerialPortStr, gpsBaud));
		simgps_interface.setTranBlock(new GPSSimParser("GPS Sim"));
		simgps_interface.setSubnet(inet_addr("0.0.0.0"));
	}

#ifdef HAVE_PICCOLO_SDK
  /*<------- Piccolo GS ------>*/
	Interface piccolo_gs("Piccolo GS");
	if(gw_piccolo_gs) {
		piccolo_gs.setCommBlock(new SocketThread("Piccolo TCP", piccoloGSCommPortStr, Socket::TCP, Socket::DUPLEX_CLIENT,piccoloGSCommIPStr,false));
		piccolo_gs.setTranBlock(new PiccoloParser("Piccolo Parser", true));
		piccolo_gs.setCompBlock(new GSBackup("Backup Server"));
		//piccolo_gs.setSubnet(inet_addr("0.0.0.0"));
	}
#endif

//  /*<---------Testing----------->*/
//  Interface testInterface("Test Interface");
//  Testing test_ui("Test UI");
//  testInterface.setCompBlock(&test_ui);
//  testInterface.setSubnet(inet_addr("0.0.0.0"));
//  exampleDaemon.link(&testInterface);

	/*<--------Ad-Hoc--------->*/
#ifdef USE_TCP_CMD
	Interface adHoc_tcp("Ad-Hoc TCP");
#endif
	Interface adHoc_udp("Ad-Hoc UDP");
	if(gw_adhoc) {
	#ifdef USE_TCP_CMD
		adHoc_tcp.setCommBlock(new SocketThread ("TCP DUPLEX", adhocPortStr_tcp, Socket::TCP, Socket::DUPLEX));
		adHoc_tcp.setSubnet(internal_subnet.s_addr);
		adHoc_tcp.setExternalIF();
		if( !gw_monitor )
			adHoc_tcp.setCanCommand();
		else
			adHoc_tcp.setCanCommand(false);
	#endif

		adHoc_udp.setCommBlock(new SocketThread ("UDP DUPLEX", adhocPortStr_udp, Socket::UDP, Socket::DUPLEX));
		//if((internal_subnet.s_addr & netmask.s_addr ) == inet_addr("6.0.0.0"))
			//adHoc_udp.setTranBlock(new AODVParser("AODV Parser"));
		adHoc_udp.setSubnet(internal_subnet.s_addr);
		adHoc_udp.setExternalIF();
		if( !gw_monitor )
			adHoc_udp.setCanCommand();
		else
			adHoc_udp.setCanCommand(false);
	}

	/*<--------Internet-------->*/
	Interface internet("Internet");
	if(gw_net) {
    if(internal_net) {
      internet.setCommBlock(new SocketThread("TCP RX", inetInPortStr, Socket::TCP, Socket::SERVER));
      internet.setCommBlock(new SocketThread("TCP TX", inetOutPortStr, Socket::TCP, Socket::LISTENING_CLIENT));
    }
    else {
      //internet.setCommBlock(new SocketThread("TCP RX", inetOutPortStr, Socket::TCP, Socket::RECEIVING_CLIENT,"20.0.0.5"));
      //internet.setCommBlock(new SocketThread("TCP TX", inetInPortStr, Socket::TCP, Socket::CLIENT,"20.0.0.5"));
      internet.setCommBlock(new SocketThread("TCP RX", inetOutPortStr, Socket::TCP, Socket::RECEIVING_CLIENT,"10.0.0.5"));
      internet.setCommBlock(new SocketThread("TCP TX", inetInPortStr, Socket::TCP, Socket::CLIENT,"10.0.0.5"));
			internet.setExternalIF();
			internet.setSubnet(inet_addr("127.0.0.0")); //FIXME should be set by interface
    }
#ifdef USING_XML
		internet.setTranBlock(new XMLtoData_Parser("XML to Data"));
		internet.setTranBlock(new DataToXML_Parser("Data to XML"));
    if(internal_net) {
			internet.setSubnet(inet_addr("0.0.0.0")); //FIXME should be set by interface
		}
#endif
		if( !gw_monitor )
			internet.setCanCommand();
	}

	/*<---------RECUV---------->*/
	Interface recuv("RECUV");
	if(gw_recuv) {
		recuv.setCommBlock(new SocketThread("RECUV TCP", recuvPortStr, Socket::TCP, Socket::DUPLEX_CLIENT,"recuv.colorado.edu"));
		//recuv.setCommBlock(new SocketThread("RECUV TCP", recuvPortStr, Socket::TCP, Socket::DUPLEX_CLIENT,"localhost"));
		//recuv.setCommBlock(new SocketThread("RECUV TCP", recuvPortStr, Socket::TCP, Socket::DUPLEX_CLIENT,"128.138.75.190"));
		recuv.setTranBlock(new GatewayParser("Gateway Parser"));
		recuv.setSubnet(inet_addr("0.0.0.0")); //FIXME should be set by interface
		recuv.setExternalIF();
	}

	/*<-------MAV Comm--------->*/
	Interface mavComm("MAV Comm");
	if(gw_mav) {
		mavComm.setCommBlock(new SerialThread("MAV Serial", mavSerialPortStr, "57600"));
		mavComm.setTranBlock(new MAVParser("MAV Parser"));
		mavComm.setSubnet(inet_addr("6.1.0.0"));
		mavComm.setExternalIF();
	}

	/*<---------RSSI---------->*/
	Interface rssi("RSSI");
	if( gw_rssi ) {
		rssi.setCommBlock(new RSSITrace("RSSI Trace"));
		rssi.setSubnet(inet_addr("255.255.255.255"));
	}

	/*<---------Media Test---------->*/
	Interface ipmt("IPMT");
	if( gw_ipmt ) {
		ipmt.setCompBlock(new IPMT("IPMT Throughput"));
		ipmt.setSubnet(inet_addr("0.0.0.0"));
	}
	
	/*<---------GPS----------->*/
	Interface gps("GPS");
	if( gw_gps ) {
		gps.setCommBlock(new SerialThread("GPS TTY", gpsSerialPortStr, gpsBaud));
		gps.setTranBlock(new GPSParser("GPS Parser"));
		gps.setSubnet(inet_addr("255.255.255.255"));
	}

  /*<--------Joystick-------->*/
  Interface joystickIF("Joystick Controller");
	if(gw_joy) {
		joystickIF.setCommBlock(new JoystickThread("Joystick Device",joySerialPortStr));
		joystickIF.setTranBlock(new JoystickTranThread("Joystick Translator"));
		joystickIF.setSubnet(inet_addr("255.255.255.255"));
	}

	/*<---Service Discovery--->*/
	SystemCharacter_t system;

	if( gw_gps ) {
		system.id = CMD_GPS;
		system.commands = 0x0;
		system.streams =
			(0x1 << ((CMD_GPS_TELEM & 0x0F) -1 ));
		gps.addCapability(&system);
	}

	if( gw_rssi ) {
		system.id = CMD_COMM;
		system.commands = 0x0;
		system.streams = (0x1 << ((CMD_LINK_STATUS & 0x0F) -1 ));
		rssi.addCapability(&system);
	}

	if( gw_ipmt ) {
		system.id = CMD_COMM;
		system.commands = (0x1 << ((CMD_RATE_TEST & 0x0F) -1 ));
		system.streams  = 0x0; 
		ipmt.addCapability(&system);
	}

	/*<-----Link and Start----->*/
#ifdef USING_GUI
	if(gw_gui) {
		system.id       = CMD_GUI;
		system.commands = 0x0;
		system.streams  = 
			(0x1 << ((CMD_GUI_CMD & 0x0F) -1 ));
		userInterface.addCapability(&system);

		exampleDaemon.link(&userInterface);
#ifdef USING_VPS
		if(gw_vps) {
			exampleDaemon.link(&vps);
		}
#endif
	}
#endif

	if(gw_adhoc) exampleDaemon.link(&adHoc_udp);
#ifdef USE_TCP_CMD
	if(gw_adhoc) exampleDaemon.link(&adHoc_tcp);
#endif
	if(gw_net)   exampleDaemon.link(&internet);
	if(gw_recuv) exampleDaemon.link(&recuv);
	if(gw_gps)   exampleDaemon.link(&gps);
	if(gw_rssi)  exampleDaemon.link(&rssi);
	if(gw_mav)   exampleDaemon.link(&mavComm);
	if(gw_ipmt)  exampleDaemon.link(&ipmt);
	//if(gw_ipmt)  exampleDaemon.link(&ipmt_udp);
  if(gw_joy)   exampleDaemon.link(&joystickIF);
#ifdef HAVE_PICCOLO_SDK
  if(gw_piccolo_gs) exampleDaemon.link(&piccolo_gs);
#endif
  if(sim_gps) exampleDaemon.link(&simgps_interface);

	exampleDaemon.start();

#ifdef USING_GUI
	if(gw_gui)  {
	#if 1
		// GTK/GDK thread enter
		gdk_threads_enter();

		gtk_main ();

		// GTK/GDK thread leave
		gdk_threads_leave();
	#else
		while( running ) {
			// GTK/GDK thread enter
			gdk_threads_enter();

			// check for gtk events
			while (gtk_events_pending())
			  gtk_main_iteration();

			// GTK/GDK thread leave
			gdk_flush();
			gdk_threads_leave();

			// sleep
			usleep( (long int)(HZ2MILLI(RUN_RATE)*100) );
		}
	#endif

	} else
#endif
		while( running ) {
		  usleep( (long int)(HZ2MILLI(RUN_RATE)*1000) );
		}

#ifdef USING_GUI
	vc_config_save ();

	// clean up any child lftp processes
	//cout << " ---- KILLING lftp"<< endl;
	int ret = ::system( "pkill lftp" );
	if( ret < 0 ) perror("system command");
#endif

	exampleDaemon.stop();

	return (EXIT_SUCCESS);
}


/***********************************************************\
|                  ExampleDaemon_Usage                       |
\************************************************************/
void ExampleDaemon_Usage(char * name)
{
	cout << "usage: " << name << " -i ID [ ARGS ]" << endl;
	cout << "\t where ARGS are: " << endl;
#ifdef HAVE_PICCOLO_SDK
	cout << "\t -a        : add interface to Piccolo GS" << endl;
#endif
	cout << "\t -i ID     : node human readable ID" << endl;
	cout << "\t -h        : display this help" << endl;
	cout << "\t -l DIR    : enable logging in DIR" << endl;
	cout << "\t -m DEVICE : mav connection on DEVICE" << endl;
	cout << "\t -n SUBNET : internal subnet" << endl;
	cout << "\t -o PORT   : TCP output port" << endl;
	cout << "\t -p PORT   : TCP input port" << endl;
	cout << "\t -u        : enable gui" << endl;
	cout << "\t -v        : verbose" << endl;
	cout << "\t -f FILE   : GPS GeoTiff map file" << endl;
	cout << "\t -c FILE   : Glade configuration file" << endl;
	cout << "\t -g        : enable gps" << endl;
	cout << "\t -r        : enable remote connection to recuv" << endl;
	cout << "\t -t        : enable wireless link trace" << endl;
	cout << "\t -b        : enable comm media test (IPMT)" << endl;
	cout << "\t -C        : disable monitor mode (i.e. enable external commanding)" << endl;
	cout << "\t -N        : disable network connection" << endl;
	cout << "\t -E        : node is making an external connection" << endl;
	cout << "\t -A        : disable wireless adhoc connection" << endl;
	cout << "\t -w NodeID : enable using wireless node of nodeID for GPS tracking " << endl;

	ExampleDaemon_Status();
}

/************************************************************\
|                  ExampleDaemon_Status                      |
\************************************************************/
void ExampleDaemon_Status()
{
//#ifdef USING_GUI
	//vc_config_init(0, NULL);
//#endif
	cout << "Example Daemon Info: "  << endl
	     << "\t PORT:ADHOC UDP     = "  << adhocPortStr_udp << endl;
#ifdef USE_TCP_CMD	
	cout << "\t PORT:ADHOC TCP     = "  << adhocPortStr_tcp << endl;
#endif 
	cout << "\t PORT:SUBNET        = "  << inet_ntoa(internal_subnet) << endl
	     << "\t PORT:INET   INPUT  = "  << inetInPortStr << endl
	     << "\t PORT:INET   OUTPUT = "  << inetOutPortStr << endl
	     << "\t PORT:SERIAL LINE   = "  << mavSerialPortStr << endl
	     << "\t PORT:SERIAL BAUD   = "  << baud << endl
	     << "\t PORT:SERIAL LINE   = "  << gpsSerialPortStr << endl
	     << "\t PORT:SERIAL BAUD   = "  << gpsBaud << endl;
#ifdef USING_GUI
	//cout << "\t FILE:GPS MAP       = "  << guiStr.config.map_file << endl
	cout << "\t FILE:Glade         = "  << guiStr.config.glade_file << endl;
#endif
}

/************************************************************\
|                  ExampleDaemon_Error                       |
\************************************************************/
void ExampleDaemon_Error(char * errstring)
{
	cerr << endl << "\tERROR - " << errstring << endl;
}

/************************************************************\
|                  ExampleDaemon_Exit                        |
\************************************************************/
void ExampleDaemon_Exit(int signal)
{
#ifdef USING_GUI
	if(gw_gui && running) 
		gtk_main_quit();
#endif

	running = false;
}

int16_t ReadByteQuick(uint8_t fd) {return -1;}

void ReverseSerialRxQueue(uint8_t chan, uint32_t Size) {}
